/*
 * PI.h
 *
 *  Created on: 2024年11月17日
 *      Author: zizhong
 */

#ifndef CONTROLLIB_PI_H_
#define CONTROLLIB_PI_H_

typedef struct FCL_cmplxCtrl_t
{
    float  ref;             // Input: reference set-point
    float  fbk;             // Input: feedback
    float  err;             // Output : error
    float  out;             // Output: controller output
    float  carryOver;       // Output : carry over for next iteration
    float  Kp;              // Parameter: proportional loop gain
    float  Ki;              // Parameter: integral gain
    float  Kerr;            // Parameter: gain for latest error
    float  KerrOld;         // Parameter: gain for prev error
    float  Umax;            // Parameter: upper saturation limit
    float  Umin;            // Parameter: lower saturation limit
    float  cosWTs;
    float  sinWTs;
    float  expVal;
    float  kDirect;
    float  xErr;
} FCL_cmplxCtrl_t;

#define FCL_CMPLXCTRL_DEFAULTS {                                               \
        0.0,        /* ref */                                                  \
        0.0,        /* fbk */                                                  \
        0.0,        /* err */                                                  \
        0.0,        /* out */                                                  \
        0.0,        /* carryOver */                                            \
        1.0,        /* Kp */                                                   \
        0.1,        /* Ki */                                                   \
        0.0,        /* Kerr */                                                 \
        0.0,        /* KerrOld */                                              \
        1.0,        /* Umax */                                                 \
        -1.0,       /* Umin */                                                 \
        0.0,        /* cosWTs */                                               \
        0.0,        /* sinWTs */                                               \
        0.0,        /* expVal */                                               \
        0.0,        /* kDirect */                                              \
        0.0         /* xErr */                                                 \
}

//
// CURRENT PI CONTROLLER ROUTINES
//
static inline void FCL_runPI(FCL_cmplxCtrl_t *pPI)
{
    pPI->out += (pPI->err * pPI->Kerr) + pPI->carryOver;
    pPI->out = __fsat(pPI->out, pPI->Umax, pPI->Umin);

    return;
}



#endif /* CONTROLLIB_PI_H_ */
